#pragma once

#ifndef _S2_H_
#define _S2_H_

#include "common_include.h"
#include"utility.h"

namespace esikf_slam{

struct S2{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  V3D vec_;   // gravity world frame
  double length_;

  S2(){
    reset();
  }

  S2(const V3D& vec):vec_(vec){
    vec_.normalized();
    vec_=vec_*length_;
  }

  void reset(){
    length_ = 9.81;
    vec_ = V3D(0,0,-1)*length_;
  }
  
	void boxplus(Eigen::Vector2d delta) {
		Eigen::Matrix<double, 3, 2> Bx;
		S2_Bx(Bx);
		V3D Bu = Bx*delta;
    SO3 res = Sophus::SO3d::exp(Bu);
		vec_ = res.matrix()* vec_;
	} 
	
	void boxminus(const S2& other, Eigen::Vector2d res) const {
		double v_sin = (Sophus::SO3d::hat(vec_)*other.vec_).norm();
		double v_cos = vec_.transpose() * other.vec_;
		double theta = std::atan2(v_sin, v_cos);
		if(v_sin < tolerance_)
		{
			if(std::fabs(theta) > tolerance_ )
			{
				res[0] = 3.1415926;
				res[1] = 0;
			}
			else{
				res[0] = 0;
				res[1] = 0;
			}
		}
		else
		{
			S2 other_copy = other;
			Eigen::Matrix<double, 3, 2>Bx;
			other_copy.S2_Bx(Bx);
			res = theta/v_sin * Bx.transpose() * Sophus::SO3d::hat(other.vec_)*vec_;
		}
	}
	
	void S2_Bx(Eigen::Matrix<double, 3, 2> &res)
	{
		if(vec_[2] + length_ > tolerance_)
		{
			res << length_ - vec_[0]*vec_[0]/(length_+vec_[2]), -vec_[0]*vec_[1]/(length_+vec_[2]),
					 -vec_[0]*vec_[1]/(length_+vec_[2]), length_-vec_[1]*vec_[1]/(length_+vec_[2]),
					 -vec_[0], -vec_[1];
			res /= length_;
		}
		else
		{
			res = Eigen::Matrix<double, 3, 2>::Zero();
			res(1, 1) = -1;
			res(2, 0) = 1;
		}
	}

	void S2_Mx(Eigen::Vector2d delta,Eigen::Matrix<double, 3, 2> &res)
	{
		Eigen::Matrix<double, 3, 2> Bx;
		S2_Bx(Bx);
		if(delta.norm() < tolerance_)
		{
			res = -Sophus::SO3d::hat(vec_)*Bx;
		}
		else{
			V3D Bu = Bx*delta;
			SO3 exp_delta=Sophus::SO3d::exp(Bu);
			res = -exp_delta.matrix()*Sophus::SO3d::hat(vec_)*vecToMatrix(Bu).transpose()*Bx;
		}
	}

  double operator[](int idx) const {return vec_[idx]; }

  S2& operator=(const S2& other) {
    if (this == &other) return *this;
    this->vec_ = other.vec_;
    this->length_ = other.length_;
    return *this;
  }

  friend std::ostream& operator<<(std::ostream &os, const S2& vec){
		return os << vec.vec_.transpose() << " ";
	}

private:
  double tolerance_ = 1e-11;
 
};



}
#endif  
